/***************************************************************************
 *   File Utility.cpp		author belot nicolas (karrakis)            *
 *   define Class Utility
 *   Some utility function		                                   *
 ***************************************************************************
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <math.h>
#include <stdlib.h>
#include "Utility.h"
#include <cassert>
#include <iostream>

#define h00(t) (2.0*(t)*(t)*(t)-3.0*(t)*(t)+1.0)
#define h01(t) (-2.0*(t)*(t)*(t)+3.0*(t)*(t))
#define h10(t) ((t)*(t)*(t)-2.0*(t)*(t)+(t))
#define h11(t) ((t)*(t)*(t)-(t)*(t))

namespace libtrckr {
void Utility::Interpolate(short type,const double* eqx, const double *eqy,unsigned  int nx, double *coeff, unsigned int max,double delta){
	if(type==MONOTONE_CUBIC){
		monotone_cubic(eqx,eqy,nx,coeff,max,delta);
	}
}

void Utility::Interpolate(short type,Buffer& b, Buffer& b2,double delta){
	if(type==MONOTONE_CUBIC){
		monotone_cubic(b,b2,delta);
	}
}

void Utility::FFT(fftw_complex *in, fftw_complex *out, unsigned int size){
	fftw_plan p = fftw_plan_dft_1d(size, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
	fftw_execute(p);
	fftw_destroy_plan(p);
}

void Utility::FFT_inverse(fftw_complex *in, fftw_complex *out, unsigned int size){
	fftw_plan p = fftw_plan_dft_1d(size, in, out, FFTW_BACKWARD, FFTW_ESTIMATE);
	fftw_execute(p);
	fftw_destroy_plan(p);
}


void Utility::monotone_cubic(const double* eqx, const double *eqy, unsigned int nx, double *coeff,unsigned int max,double delta){
	/// \a http://en.wikipedia.org/wiki/Monotone_cubic_interpolation
	#define M 0
	#define D 1
	#define A 2
	#define B 3
	//#define place(a,b) (a)*nx+(b)
	//#define place(a,b) (b)*4+(a)
	#define place(a,b) ( (b) << 2) + (a)
	/* Old code double* mx=(double*)malloc(sizeof(double) * nx);
	double* dx=(double*)malloc(sizeof(double) * nx);
	double* ax=(double*)malloc(sizeof(double) * nx);
	double* bx=(double*)malloc(sizeof(double) * nx);*/
	double *tangeante=(double*)malloc(sizeof(double)*4*nx);

	// Calculate the m of each point 
			
	for(unsigned int i=0;i<nx;i++){
		if(i==0 ){
			//old code
			//mx[i]=(eqy[i+1]-eqy[i])/(eqx[i+1]-eqx[i]);
			tangeante[place(M,i)]=(eqy[i+1]-eqy[i])/(eqx[i+1]-eqx[i]);
		}else if(i>=nx-1){
			if(i<nx){
				//old code
				//mx[i]=(eqy[i]-eqy[i-1])/(eqx[i]-eqx[i-1]);
				tangeante[place(M,i)]=(eqy[i]-eqy[i-1])/(eqx[i]-eqx[i-1]);
			}
		}else{
			//old code
			//mx[i]=(eqy[i]-eqy[i-1])/(2*(eqx[i]-eqx[i-1])) + (eqy[i+1]-eqy[i])/(2*(eqx[i+1]-eqx[i]));
			tangeante[place(M,i)]=(eqy[i]-eqy[i-1])/(2*(eqx[i]-eqx[i-1])) + (eqy[i+1]-eqy[i])/(2*(eqx[i+1]-eqx[i]));
		}
		//std::cout << place(M,i) << tangeante[place(M,i)] << std::endl;
	}
	//calulate the delta of each point and the alpha and beta.	
	for(unsigned int i=0;i<nx;i++){
		if(i<nx-1){ 
			/*Old code
			dx[i]=(eqy[i+1] - eqy[i])/(eqx[i+1]-eqx[i]);
			if(dx[i]==0.0){
				mx[i] = mx[i+1] = 0.0;
			}
			ax[i]=mx[i]/dx[i];
			bx[i]=mx[i+1]/dx[i];	*/
			tangeante[place(D,i)]=(eqy[i+1] - eqy[i])/(eqx[i+1]-eqx[i]);
			if(tangeante[place(D,i)]==0.0){
				tangeante[place(M,i)] = tangeante[place(M,i+1)]=0.0;
			}
			tangeante[place(A,i)]=tangeante[place(M,i)]/tangeante[place(M,i)];
			tangeante[place(B,i)]=tangeante[place(M,i+1)]/tangeante[place(M,i)];
		}
		//if (a²+b² > 9 special handler.. 
		/*old code 
		if( i<nx-1 && ax[i]*ax[i]+bx[i]*bx[i] > 9){
			double tau = 3* pow(ax[i]*ax[i] + bx[i]*bx[i],-0.5);
			mx[i]=tau * ax[i] * dx[i];
			mx[i+1]=tau * ax[i] * dx[i];
		}*/
		if(i<nx-1 && tangeante[place(A,i)]*tangeante[place(A,i)] + tangeante[place(B,i)]*tangeante[place(B,i)] > 9){
			double tau = 3* pow(tangeante[place(A,i)]*tangeante[place(A,i)] + tangeante[place(B,i)]*tangeante[place(B,i)],-0.5);
			tangeante[place(M,i)]=tau * tangeante[place(A,i)] * tangeante[place(D,i)];
			tangeante[place(M,i+1)]=tau * tangeante[place(A,i)] * tangeante[place(D,i)];
		}


		//std::cout << mx[i] << " " << dx[i] << " " << ax[i] << " " << bx[i] << std::endl; 
	}
	//finding the 2 points encalupsating (???) the point i
	//And calculating the interpolation
	// h00, h10, h01, h11 are the spline functions.
	for(unsigned int i=0;i<max;i++){
		int lo=0;
		int up=0;
		/* Old code
			check for every j.
		for(unsigned int j =0;j<nx;j++){
			if(eqx[j]<i*delta){
				lo=j;
			}
		}
		for(int j=nx-1;j>=0;j--){
			if(eqx[j]>i*delta){
				up=j;
			}
		}
		if(eqx[up]<i*delta){
			up=nx-1;
		}*/

		//new code
		for(unsigned int j =0; j<nx && eqx[j]<i*delta; ++j){ lo=j;}
		/*for(unsigned int j =0;j<nx;j++){
			if(eqx[j]<i*delta){
				lo=j;
			}
			else { break; }
		}*/
		up=lo+1;
		if(lo>=nx-1){
			up=nx-1;
			if(lo>=nx)
				lo=nx-1;
		}
		
		double h=eqx[up]-eqx[lo];
		double t= (i*delta - eqx[lo])/h;
		//std::cout << i << " " << lo << " " << up << std::endl;
		if(up==lo)t=0.0;
		if(lo==0 && up==0){
			coeff[i]=eqy[0];
		}else if(lo==nx && up==nx){
			coeff[i]=eqy[nx];
		}else{
			coeff[i]=eqy[lo]*h00(t) + h*tangeante[place(M,lo)]*h10(t) + eqy[up]*h01(t) + h*tangeante[place(M,up)]*h11(t);
		}
	}
	/*old code
	free(mx);
	free(dx);
	free(ax);
	free(bx);*/
	free(tangeante);
}
void Utility::monotone_cubic( Buffer& b , Buffer& b2, double delta){
	uint nx = b.size();
	uint max = b2.size();
	double *eqx = (double*)malloc(sizeof(double)*nx);
	for(uint i=0;i<nx;i++){
		eqx[i]=i;
	}
	/// \a http://en.wikipedia.org/wiki/Monotone_cubic_interpolation
	#define M 0
	#define D 1
	#define A 2
	#define B 3
	//#define place(a,b) (a)*nx+(b)
	//#define place(a,b) (b)*4+(a)
	#define place(a,b) ( (b) << 2) + (a)
	/* Old code double* mx=(double*)malloc(sizeof(double) * nx);
	double* dx=(double*)malloc(sizeof(double) * nx);
	double* ax=(double*)malloc(sizeof(double) * nx);
	double* bx=(double*)malloc(sizeof(double) * nx);*/
	b.setChannel(0);
	b2.setChannel(0);
	double *tangeante=(double*)malloc(sizeof(double)*4*nx);
	while(b.hasNextChannel()){


	// Calculate the m of each point 
			
	for(unsigned int i=0;i<nx;i++){
		if(i==0 ){
			//old code
			//mx[i]=(eqy[i+1]-eqy[i])/(eqx[i+1]-eqx[i]);
			tangeante[place(M,i)]=(b[i+1]-b[i])/(eqx[i+1]-eqx[i]);
		}else if(i>=nx-1){
			if(i<nx){
				//old code
				//mx[i]=(eqy[i]-eqy[i-1])/(eqx[i]-eqx[i-1]);
				tangeante[place(M,i)]=(b[i]-b[i-1])/(eqx[i]-eqx[i-1]);
			}
		}else{
			//old code
			//mx[i]=(eqy[i]-eqy[i-1])/(2*(eqx[i]-eqx[i-1])) + (eqy[i+1]-eqy[i])/(2*(eqx[i+1]-eqx[i]));
			tangeante[place(M,i)]=(b[i]-b[i-1])/(2*(eqx[i]-eqx[i-1])) + (b[i+1]-b[i])/(2*(eqx[i+1]-eqx[i]));
		}
		//std::cout << place(M,i) << tangeante[place(M,i)] << std::endl;
	}
	//calulate the delta of each point and the alpha and beta.	
	for(unsigned int i=0;i<nx;i++){
		if(i<nx-1){ 
			/*Old code
			dx[i]=(eqy[i+1] - eqy[i])/(eqx[i+1]-eqx[i]);
			if(dx[i]==0.0){
				mx[i] = mx[i+1] = 0.0;
			}
			ax[i]=mx[i]/dx[i];
			bx[i]=mx[i+1]/dx[i];	*/
			tangeante[place(D,i)]=(b[i+1] - b[i])/(eqx[i+1]-eqx[i]);
			if(tangeante[place(D,i)]==0.0){
				tangeante[place(M,i)] = tangeante[place(M,i+1)]=0.0;
			}
			tangeante[place(A,i)]=tangeante[place(M,i)]/tangeante[place(M,i)];
			tangeante[place(B,i)]=tangeante[place(M,i+1)]/tangeante[place(M,i)];
		}
		//if (a²+b² > 9 special handler.. 
		/*old code 
		if( i<nx-1 && ax[i]*ax[i]+bx[i]*bx[i] > 9){
			double tau = 3* pow(ax[i]*ax[i] + bx[i]*bx[i],-0.5);
			mx[i]=tau * ax[i] * dx[i];
			mx[i+1]=tau * ax[i] * dx[i];
		}*/
		if(i<nx-1 && tangeante[place(A,i)]*tangeante[place(A,i)] + tangeante[place(B,i)]*tangeante[place(B,i)] > 9){
			double tau = 3* pow(tangeante[place(A,i)]*tangeante[place(A,i)] + tangeante[place(B,i)]*tangeante[place(B,i)],-0.5);
			tangeante[place(M,i)]=tau * tangeante[place(A,i)] * tangeante[place(D,i)];
			tangeante[place(M,i+1)]=tau * tangeante[place(A,i)] * tangeante[place(D,i)];
		}


		//std::cout << mx[i] << " " << dx[i] << " " << ax[i] << " " << bx[i] << std::endl; 
	}
	//finding the 2 points encalupsating (???) the point i
	//And calculating the interpolation
	// h00, h10, h01, h11 are the spline functions.
	for(unsigned int i=0;i<max;i++){
		int lo=0;
		int up=0;
		/* Old code
			check for every j.
		for(unsigned int j =0;j<nx;j++){
			if(eqx[j]<i*delta){
				lo=j;
			}
		}
		for(int j=nx-1;j>=0;j--){
			if(eqx[j]>i*delta){
				up=j;
			}
		}
		if(eqx[up]<i*delta){
			up=nx-1;
		}*/

		//new code
		for(unsigned int j =0; j<nx && eqx[j]<i*delta; ++j){ lo=j;}
		/*for(unsigned int j =0;j<nx;j++){
			if(eqx[j]<i*delta){
				lo=j;
			}
			else { break; }
		}*/
		up=lo+1;
		if(lo>=nx-1){
			up=nx-1;
			if(lo>=nx)
				lo=nx-1;
		}
		
		double h=eqx[up]-eqx[lo];
		double t= (i*delta - eqx[lo])/h;
		//std::cout << i << " " << lo << " " << up << std::endl;
		if(up==lo)t=0.0;
		if(lo==0 && up==0){
			b2[i]=b[0];
		}else if(lo==nx && up==nx){
			b2[i]=b2[nx-1];
		}else{
			b2[i]=b[lo]*h00(t) + h*tangeante[place(M,lo)]*h10(t) + b[up]*h01(t) + h*tangeante[place(M,up)]*h11(t);
		}
	}
	b.nextChannel();
	b2.nextChannel();
	}
	/*old code
	free(mx);
	free(dx);
	free(ax);
	free(bx);*/
	free(tangeante);
}
/*double Utility::h00(double t){
	return 2.0*t*t*t -3*t*t+1;
}

double Utility::h10(double t){
	return t*t*t-2*t*t+t;
}

double Utility::h01(double t){
	return -2 * t *t *t +3*t*t;
}

double Utility::h11(double t){
	return t*t*t - t*t;
}
*/
}

